package ar.edu.untref.robotica.simulador;

public class DetectorDeChoque {

	private double posicionX;
	private double posicionY;

	public DetectorDeChoque(double posicionX, double posicionY) {
		this.posicionX = posicionX;
		this.posicionY = posicionY;
	}
	
	public boolean hayChoque(Escenario escenario){
		double hipo = Math.sqrt(Math.pow(posicionX, 2)+ Math.pow(posicionY, 2));
		double anguloRelativo = Math.atan(posicionX/posicionY)*-1;
		double anguloAbsoluto = Math.toRadians(escenario.getPosicionRobot().getAngulo()) + anguloRelativo;

		double posicionActualDetectorY;
		double posicionActualDetectorX;
		
		//cambia si esta debajo del eje Y
		if(posicionY < 0){
			posicionActualDetectorY = escenario.getPosicionRobot().getPosicionY() - hipo* Math.cos(anguloAbsoluto);
			posicionActualDetectorX = escenario.getPosicionRobot().getPosicionX() + hipo* Math.sin(anguloAbsoluto);
		}else{
			posicionActualDetectorY = escenario.getPosicionRobot().getPosicionY() + hipo* Math.cos(anguloAbsoluto);
			posicionActualDetectorX = escenario.getPosicionRobot().getPosicionX() - hipo* Math.sin(anguloAbsoluto);
		}
		return (escenario.getMatriz()[(int) posicionActualDetectorX][(int) posicionActualDetectorY] == 1);
	}

}
